/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* Copyright (C) 2004
 *   Toby Collett, University of Auckland Robotics Group
 */


/** @ingroup drivers */
/** @{ */
/** @defgroup driver_khepera khepera
 * @brief K-Team Khepera mobile robot

The khepera driver is used to interface to the K-Team khepera robot. 

This driver is experimental and should be treated with caution. At
this point it supports the @ref interface_position2d and 
@ref interface_ir interfaces.

TODO: 
 - Add support for position control (currently only velocity control)
 - Add proper calibration for IR sensors

@par Compile-time dependencies

- none

@par Provides

- @ref interface_position2d
- @ref interface_ir

@par Requires

- none

@par Supported configuration requests

- The @ref interface_position2d interface supports:
  - PLAYER_POSITION_GET_GEOM_REQ
  - PLAYER_POSITION_MOTOR_POWER_REQ
  - PLAYER_POSITION_VELOCITY_MODE_REQ
  - PLAYER_POSITION_RESET_ODOM_REQ
  - PLAYER_POSITION_SET_ODOM_REQ
- The @ref interface_ir interface supports:
  - PLAYER_IR_POSE_REQ

@par Configuration file options

- port (string)
  - Default: "/dev/ttyUSB0"
  - Serial port used to communicate with the robot.
- scale_factor (float)
  - Default: 10
  - As the khepera is so small the actual geometry doesnt make much sense
    with many of the existing defaults so the geometries can all be scaled
    by this factor.
- encoder_res (float)
  - Default: 1.0/12.0
  - The wheel encoder resolution.
- position_pose (float tuple)
  - Default: [0 0 0]
  - The pose of the robot in player coordinates (mm, mm, deg).
- position_size (float tuple)
  - Default: [57 57]
  - The size of the robot approximated to a rectangle (mm, mm).
- ir_pose_count (integer)
  - Default: 8
  - The number of ir poses.
- ir_poses (float tuple)
  - Default: [10 24 90 19 17 45 25 6 0 25 -6 0 19 -17 -45 10 -24 -90 -24 -10 180 -24 10 180]
  - Poses of the IRs (mm mm deg for each one)

@par Example 

@verbatim
driver
(
  name "khepera"
  provides ["position2d:0" "ir:0"]
)
@endverbatim

@author Toby Collett
*/
/** @} */

#include <fcntl.h>
#include <signal.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <math.h>
#include <stdlib.h>  /* for abs() */
#include <netinet/in.h>
#include <ctype.h>

#include <assert.h>

#include <khepera.h>

#include <libplayercore/playercore.h>

// we need to debug different things at different times
//#define DEBUG_POS
//#define DEBUG_SERIAL
#define DEBUG_CONFIG

// useful macros
#define DEG2RAD(x) (((double)(x))*0.01745329251994)
#define RAD2DEG(x) (((double)(x))*57.29577951308232)

//#define DEG2RAD_FIX(x) ((x) * 17453)
//#define RAD2DEG_FIX(x) ((x) * 57295780)
#define DEG2RAD_FIX(x) ((x) * 174)
#define RAD2DEG_FIX(x) ((x) * 572958)


/* initialize the driver.
 *
 * returns: pointer to new REBIR object
 */
Driver*
Khepera_Init(ConfigFile *cf, int section)
{
  return (Driver *) new Khepera( cf, section);
}

/* register the Khepera IR driver in the drivertable
 *
 * returns: 
 */
void
Khepera_Register(DriverTable *table) 
{
  table->AddDriver("khepera", Khepera_Init);
}

int Khepera::ProcessMessage(MessageQueue * resp_queue, player_msghdr * hdr, void * data)
{
	assert(hdr);
	assert(data);

	//Request IR Geometry (Pose)
	if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_IR_POSE, ir_addr))
	{
		Publish(ir_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geometry->ir, sizeof(geometry->ir));
		return 0;
	}
	//Request Sonar Geometry (Pose)
	if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SONAR_REQ_GET_GEOM, sonar_addr))
	{
		Publish(sonar_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geometry->sonar, sizeof(geometry->sonar));
		return 0;
	}
	//Request: Bumper Geometry
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_BUMPER_GET_GEOM, bumper_addr))
	{
		Publish(bumper_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geometry->bumpers, sizeof(geometry->bumpers));
		return 0;
	}
	
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_SOUND_CMD_IDX, sound_addr))
	{
		player_sound_cmd_t *soundClip = reinterpret_cast<player_sound_cmd_t *>(data);
		PlayJingle(soundClip->index);
		return 0;
	}
	
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, position_addr))
	{
		player_position2d_cmd_vel_t * poscmd = reinterpret_cast<player_position2d_cmd_vel_t *> (data);


		//FIXME: sera necesario esto de actualizar la posicion en este punto?
		#if CALCULATE_ODOMETRY == 1
    player_position2d_data_t position_data;

		UpdatePosData(&position_data);

  	//put position data
	  Publish(position_addr,NULL,PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, (unsigned char *) &position_data, sizeof(player_position2d_data_t),NULL);
	  #endif
	  
		// need to calculate the left and right velocities
		float transvel = static_cast<float>(poscmd->vel.px);
		float rotvel = static_cast<float>(poscmd->vel.pa);		
		
		float left_vel = static_cast<float>(transvel - rotvel);
		float right_vel = static_cast<float>(transvel + rotvel);
		
		if(left_vel>1)
			left_vel=1;
			
		if(right_vel>1)
			right_vel=1;
			
		if(left_vel<-1)
			left_vel=-1;
			
		if(right_vel<-1)
			right_vel=-1;
			
		int leftvel = (int) (left_vel*NXBOT_MAX_VEL);
		int rightvel = (int) (right_vel*NXBOT_MAX_VEL);
		
		if (this->motors_enabled) 

			SetSpeed(leftvel,rightvel);
		else 

			SetSpeed(0,0);
		return 0;

	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_GET_GEOM, position_addr))
	{
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, hdr->subtype, &geometry->position, sizeof(geometry->position));
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_MOTOR_POWER, position_addr))
	{
		this->motors_enabled = ((player_position2d_power_config_t *)data)->state;
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,hdr->subtype);
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_VELOCITY_MODE, position_addr))
	{
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,hdr->subtype);
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_RESET_ODOM, position_addr))
	{
		ResetOdometry();
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,hdr->subtype);
		return 0;
	}
	else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_POSITION2D_REQ_SET_ODOM, position_addr))
	{
		player_position2d_set_odom_req_t * odomCmd = reinterpret_cast<player_position2d_set_odom_req_t *> (data);
		player_pose_t newPose = odomCmd->pose;
		//FIXME
		#if CALCULATE_ODOMETRY == 0
	  	SetPos(newPose.px, newPose.py, newPose.pa);
	  #else
	  	//reset odometers in nxbot
	  	SetPosCounter(0,0);
	  	x = newPose.px;
	  	y = newPose.py;
	  	yaw = newPose.pa;
	  	//reset last values of encoders
	  	last_lpos = 0;
  		last_rpos = 0;
	  	
	  	
	  #endif
		Publish(position_addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK,hdr->subtype);
		return 0;
	}

	return -1;
}

Khepera::Khepera(ConfigFile *cf, int section) : Driver(cf, section)
{
  // zero ids, so that we'll know later which interfaces were requested
  memset(&position_addr, 0, sizeof(this->position_addr));
  memset(&ir_addr, 0, sizeof(ir_addr));
  memset(&power_addr, 0, sizeof(power_addr));
  memset(&bumper_addr, 0, sizeof(bumper_addr));
  memset(&sound_addr, 0, sizeof(sound_addr));
  memset(&sonar_addr, 0, sizeof(sonar_addr));

  this->position_subscriptions = this->ir_subscriptions = this->power_subscriptions = this->bumper_subscriptions = this->sound_subscriptions = this->sonar_subscriptions = 0;

  // Do we create a robot position interface?
  if(cf->ReadDeviceAddr(&(this->position_addr), section, "provides", 
                      PLAYER_POSITION2D_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->position_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }

  // Do we create an ir interface?
  if(cf->ReadDeviceAddr(&(this->ir_addr), section, "provides", 
                      PLAYER_IR_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->ir_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }
  
  // Do we create a power interface?
  if(cf->ReadDeviceAddr(&(this->power_addr), section, "provides", 
                      PLAYER_POWER_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->power_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }

  // Do we create a bumper interface?
  if(cf->ReadDeviceAddr(&(this->bumper_addr), section, "provides", 
                      PLAYER_BUMPER_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->bumper_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }
  
  // Do we create a sound interface?
  if(cf->ReadDeviceAddr(&(this->sound_addr), section, "provides", 
                      PLAYER_SOUND_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->sound_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }
  
  // Do we create a sonar interface?
  if(cf->ReadDeviceAddr(&(this->sonar_addr), section, "provides", 
                      PLAYER_SONAR_CODE, -1, NULL) == 0)
  {
    if(this->AddInterface(this->sonar_addr) != 0)
    {
      this->SetError(-1);    
      return;
    }
  }
  
  
  // Read config file options
  geometry = new player_nxbot_geom_t;


  geometry->PortName = strdup(cf->ReadString(section, "port", NXBOT_DEFAULT_SERIAL_PORT));

  //geometry->encoder_res = cf->ReadFloat(section,"encoder_res", NXBOT_DEFAULT_ENCODER_RES);
  
  //load odometry parameters
  geometry->odometry.wheelRadius = cf->ReadTupleFloat(section,"odometry_param",0,NXBOT_WHEEL_RADIUS);
  if(geometry->odometry.wheelRadius == -1)
  {
  	geometry->odometry.wheelRadius = NXBOT_WHEEL_RADIUS;
  	geometry->encoder_res = NXBOT_DEFAULT_ENCODER_RES;
  }
  else if (geometry->odometry.wheelRadius == NXBOT_WHEEL_RADIUS)
  	geometry->encoder_res = NXBOT_DEFAULT_ENCODER_RES;
  else
  	geometry->encoder_res = 2* M_PI * geometry->odometry.wheelRadius / NXBOT_ENCODER_MAX_COUNT;
  
  geometry->odometry.wheelDist = cf->ReadTupleFloat(section,"odometry_param",1,NXBOT_WHEEL_DISTANCE);
  if(geometry->odometry.wheelDist == -1)
  	geometry->odometry.wheelDist = NXBOT_WHEEL_DISTANCE;
  	
  geometry->odometry.k1 = cf->ReadTupleFloat(section,"odometry_param",2,1);
  geometry->odometry.k2 = cf->ReadTupleFloat(section,"odometry_param",3,1);
  geometry->odometry.k3 = cf->ReadTupleFloat(section,"odometry_param",4,1);
  
	printf("R: %f W: %f k1:%f k2:%f k3:%f\n",geometry->odometry.wheelRadius,geometry->odometry.wheelDist, geometry->odometry.k1, geometry->odometry.k2, geometry->odometry.k3);
  // Load position config
  geometry->position.pose.px = cf->ReadTupleFloat(section,"position_pose",0,0);
  geometry->position.pose.py = cf->ReadTupleFloat(section,"position_pose",1,0);
  geometry->position.pose.pa = cf->ReadTupleFloat(section,"position_pose",2,0);

  // load dimension of the base
  geometry->position.size.sw = cf->ReadTupleFloat(section,"position_size",0,0.215);
  geometry->position.size.sl = cf->ReadTupleFloat(section,"position_size",1,0.215);

  // load ir geometry config
  geometry->ir.poses_count = (cf->ReadInt(section,"ir_pose_count", 6));
  
  if (geometry->ir.poses_count == 6 && cf->ReadTupleFloat(section,"ir_poses",0,-1) == -1)
  {
    // load the default ir geometry of nxbot
    geometry->ir.poses[0].px = 0;
    geometry->ir.poses[0].py = 0.1;
    geometry->ir.poses[0].pa = DTOR(90);

    geometry->ir.poses[1].px = 0.07;
    geometry->ir.poses[1].py = 0.07;
    geometry->ir.poses[1].pa = DTOR(45);

    geometry->ir.poses[2].px = 0.1;
    geometry->ir.poses[2].py = 0.017;
    geometry->ir.poses[2].pa = DTOR(10);

    geometry->ir.poses[3].px = 0.1;
    geometry->ir.poses[3].py = -0.017;
    geometry->ir.poses[3].pa = DTOR(-10);

    geometry->ir.poses[4].px = 0.07;
    geometry->ir.poses[4].py = -0.07;
    geometry->ir.poses[4].pa = DTOR(-45);

    geometry->ir.poses[5].px = 0;
    geometry->ir.poses[5].py = -0.1;
    geometry->ir.poses[5].pa = DTOR(-90);
  }
  else
  {
    // laod geom from config file
    for (unsigned int i = 0; i < geometry->ir.poses_count; ++i)
    {
      geometry->ir.poses[i].px = cf->ReadTupleFloat(section,"ir_poses",3*i,0);
      geometry->ir.poses[i].py = cf->ReadTupleFloat(section,"ir_poses",3*i+1,0);
      geometry->ir.poses[i].pa = cf->ReadTupleFloat(section,"ir_poses",3*i+2,0);
    }				
  }
  
  // load sonar geometry config
  //first read number of sonars
  geometry->sonar.poses_count = (cf->ReadInt(section,"sonar_pose_count", 3));
  
  //now read poses 
  if (geometry->sonar.poses_count == 3 && cf->ReadTupleFloat(section,"sonar_poses",0,-1) == -1)
  {
    // load the default sonar geometry of nxbot
    geometry->sonar.poses[0].px = 0.07;
    geometry->sonar.poses[0].py = 0.07;
    geometry->sonar.poses[0].pa = DTOR(45);

    geometry->sonar.poses[1].px = 0.1075;
    geometry->sonar.poses[1].py = 0;
    geometry->sonar.poses[1].pa = DTOR(0);


    geometry->sonar.poses[2].px = 0.07;
    geometry->sonar.poses[2].py = -0.07;
    geometry->sonar.poses[2].pa = DTOR(-45);

  }
  else
  {
    // load geom from config file
    for (unsigned int i = 0; i < geometry->sonar.poses_count; ++i)
    {
      geometry->sonar.poses[i].px = cf->ReadTupleFloat(section,"sonar_poses",3*i,0);
      geometry->sonar.poses[i].py = cf->ReadTupleFloat(section,"sonar_poses",3*i+1,0);
      geometry->sonar.poses[i].pa = cf->ReadTupleFloat(section,"sonar_poses",3*i+2,0);
    }				
  }
  
  geometry->bumpers.bumper_def_count = 4;
  
  if (geometry->bumpers.bumper_def_count == 4 && cf->ReadTupleFloat(section,"bumper_poses",0,-1) == -1)
  {
    // load the default bumper geometry of nxbot
    geometry->bumpers.bumper_def[0].pose.px = 0.07f;
    geometry->bumpers.bumper_def[0].pose.py = 0.07f;
    geometry->bumpers.bumper_def[0].pose.pa = DTOR(45);
    geometry->bumpers.bumper_def[0].length = 0.1f;
    geometry->bumpers.bumper_def[0].radius = 0.2f;
    
    geometry->bumpers.bumper_def[1].pose.px = 0.07f;
    geometry->bumpers.bumper_def[1].pose.py = -0.07f;
    geometry->bumpers.bumper_def[1].pose.pa = DTOR(-45);
    geometry->bumpers.bumper_def[1].length = 0.1f;
    geometry->bumpers.bumper_def[1].radius = 0.2f;
    
    geometry->bumpers.bumper_def[2].pose.px = -0.07f;
    geometry->bumpers.bumper_def[2].pose.py = 0.07f;
    geometry->bumpers.bumper_def[2].pose.pa = DTOR(135);
    geometry->bumpers.bumper_def[2].length = 0.1f;
    geometry->bumpers.bumper_def[2].radius = 0.2f;
    
    geometry->bumpers.bumper_def[3].pose.px = -0.07f;
    geometry->bumpers.bumper_def[3].pose.py = -0.07f;
    geometry->bumpers.bumper_def[3].pose.pa = DTOR(-135);
    geometry->bumpers.bumper_def[3].length = 0.1f;
    geometry->bumpers.bumper_def[3].radius = 0.2f;
    
  }

  //reset position counters
  last_lpos = 0;
  last_rpos = 0;
  
  
}

int 
Khepera::Subscribe(player_devaddr_t addr)
{
  int setupResult;

  // do the subscription
  if((setupResult = Driver::Subscribe(addr)) == 0)
  {
    
    // also increment the appropriate subscription counter
    switch(addr.interf)
    {
      case PLAYER_POSITION2D_CODE:
        this->position_subscriptions++;
        break;
      case PLAYER_IR_CODE:
        this->ir_subscriptions++;
        break;
      case PLAYER_POWER_CODE:
      	this->power_subscriptions++;
      	break;
      case PLAYER_BUMPER_CODE:
      	this->bumper_subscriptions++;
      	break;
      case PLAYER_SOUND_CODE:
      	this->sound_subscriptions++;
      	break;
      case PLAYER_SONAR_CODE:
      	this->sonar_subscriptions++;
      	break;
      		
      
    }
  }

  return(setupResult);
}

int 
Khepera::Unsubscribe(player_devaddr_t addr)
{
  int shutdownResult;
  // do the unsubscription
  if((shutdownResult = Driver::Unsubscribe(addr)) == 0)
  {
    
    // also decrement the appropriate subscription counter
    switch(addr.interf)
    {
      case PLAYER_POSITION2D_CODE:
        assert(--this->position_subscriptions >= 0);
        break;
      case PLAYER_IR_CODE:
        assert(--this->ir_subscriptions >= 0);
        break;
      case PLAYER_POWER_CODE:
        assert(--this->power_subscriptions >= 0);
        break;
      case PLAYER_BUMPER_CODE:
        assert(--this->bumper_subscriptions >= 0);
        break;
      case PLAYER_SOUND_CODE:
        assert(--this->sound_subscriptions >= 0);
        break;
      case PLAYER_SONAR_CODE:
        assert(--this->sonar_subscriptions >= 0);
        break;
      
    }
  }

  return(shutdownResult);
}

/* called the first time a client connects
 *
 * returns: 0 on success
 */
int 
Khepera::Setup()
{
  // open and initialize the serial to -> Khepera  
  printf("NX-Bot: connection initializing (%s)...", geometry->PortName);
  fflush(stdout);
  Serial = new KheperaSerial(geometry->PortName,NXBOT_BAUDRATE);
  if (Serial == NULL || !Serial->Open())
  {
    return 1;
  }
  printf("Done\n");

  motors_enabled = false;

  //Indicate connection
  PlayJingle(0);
  /* now spawn reading thread */
  StartThread();
  return(0);
}


int 
Khepera::Shutdown()
{
  printf("NX-Bot: SHUTDOWN\n");
  
  //Lock();
  SetSpeed(0,0);
  //indicate disconnection
  PlayJingle(1);
  //Unlock();
  
  StopThread();

  // Killing the thread seems to leave out serial
  // device in a bad state, need to fix this,
  // till them we just dont stop the robot
  // which is theoretically quite bad...but this is the khepera...
  // it cant do that much damage its only 7cm across
  
  delete Serial;
  Serial = NULL;

  return(0);
}

void 
Khepera::Main()
{
  int last_position_subscrcount=0;

  pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);

  while (1) 
  {
    // we want to reset the odometry and enable the motors if the first 
    // client just subscribed to the position device, and we want to stop 
    // and disable the motors if the last client unsubscribed.
    
    if(!last_position_subscrcount && this->position_subscriptions)
    {
      printf("NX-Bot: first pos sub. turn off and reset\n");
      // then first sub for pos, so turn off motors and reset odom
      SetSpeed(0,0);
      if(ResetOdometry()==-1)
      	printf("There was an error reseting odometry\n");

    } 
    else if(last_position_subscrcount && !(this->position_subscriptions))
    {
      // last sub just unsubbed
      
      printf("NX-Bot: last pos sub gone\n");
      SetSpeed(0,0);
    }
    else if(this->position_subscriptions || this->ir_subscriptions || this->bumper_subscriptions || this->power_subscriptions || this->sound_subscriptions  || this->sonar_subscriptions)
    {
    
    }
    last_position_subscrcount = this->position_subscriptions;


	ProcessMessages();
    pthread_testcancel();

    // now lets get new data...
    UpdateData();

    pthread_testcancel();
  }
  pthread_exit(NULL);
}



/* this will update the data that is sent to clients
 * just call separate functions to take care of it
 *
 * returns:
 */
void
Khepera::UpdateData()
{
  player_position2d_data_t position_data;
  player_ir_data_t ir_data;
  player_sonar_data_t sonar_data;
  player_power_data_t power_data;
  player_bumper_data_t bumper_data;
  
  UpdatePosData(&position_data);

  // put position data
  Publish(position_addr,NULL,PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, (unsigned char *) &position_data, sizeof(player_position2d_data_t),NULL);

  // put ir data
  UpdateIRData(&ir_data);

  Publish(ir_addr,NULL,PLAYER_MSGTYPE_DATA, PLAYER_IR_DATA_RANGES, (unsigned char *) &ir_data, sizeof(ir_data),NULL);
  
  // put sonar data
  UpdateSonarData(&sonar_data);

  Publish(sonar_addr,NULL,PLAYER_MSGTYPE_DATA, PLAYER_SONAR_DATA_RANGES, (unsigned char *) &sonar_data, sizeof(sonar_data),NULL);
  
  
  // put power data
  updatePowerData(&power_data);
  
  Publish(power_addr,NULL,PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE, (unsigned char *) &power_data, sizeof(power_data),NULL);
  
  // put bumper data
  updateBumperData(&bumper_data);
  
  Publish(bumper_addr,NULL,PLAYER_MSGTYPE_DATA, PLAYER_BUMPER_DATA_STATE, (unsigned char *) &bumper_data, sizeof(bumper_data),NULL);
  

}

/* this will update the IR part of the client data
 * it entails reading the currently active IR sensors
 * and then changing their state to off and turning on
 * 2 new IRs.  
 *
 * returns:
 */

void
Khepera::UpdateIRData(player_ir_data_t * d)
{
  ReadAllIR(d);

  d->ranges_count = geometry->ir.poses_count;
  d->voltages_count = geometry->ir.poses_count;
  
  for (unsigned int i =0; i < geometry->ir.poses_count; i++) 
  {
  //en ranges poner la dist max a la cual se detectan obst (una constante pero que se pueda pasar como param)
  // para voltages poner simpleente 0 o 1
    d->ranges[i] = (1 - d->voltages[i]);
    //d->voltages[i] = d->voltages[i];
  }
}

void Khepera::UpdateSonarData(player_sonar_data_t *pd)
{
  int sonarValues[3];

	pd->ranges_count = geometry->sonar.poses_count;
	
  //send command. if it fails, return -1  
  if(Serial->KheperaCommand('W',0,NULL,3,sonarValues) >= 0)
  {
  	//ranges are in cm.
    pd->ranges[0] = ((float)sonarValues[0]/100);
    pd->ranges[1] = ((float)sonarValues[1]/100);
    pd->ranges[2] = ((float)sonarValues[2]/100);
    
    
  }

}

void Khepera::updatePowerData(player_power_data_t *pd)
{
  int powerValues[2];

  //send command. if it fails, return -1  
  if(Serial->KheperaCommand('Q',0,NULL,2,powerValues) >= 0)
  {
    pd->valid = PLAYER_POWER_MASK_VOLTS | PLAYER_POWER_MASK_PERCENT;
    pd->volts = powerValues[1];
    pd->percent = powerValues[0];
    
    pd->volts /=100;
    
  }

}

void Khepera::updateBumperData(player_bumper_data_t *bd)
{
  int bumperValues;

  //send command. if it fails, return -1  
  if(Serial->KheperaCommand('M',0,NULL,1,&bumperValues) >= 0)
  {
    bd->bumpers_count = 4;
    bd->bumpers[0] = bumperValues & 0x01;
    bd->bumpers[1] = (bumperValues & 0x02)>>1;
    bd->bumpers[2] = (bumperValues & 0x04)>>2;
    bd->bumpers[3] = (bumperValues & 0x08)>>3;
    
    
  }

}

  
/* this will update the position data.  this entails odometry, etc
 */ 
void
Khepera::UpdatePosData(player_position2d_data_t *d)
{
	#if CALCULATE_ODOMETRY == 0
	float xx;
	float yy;
	float yyaw;
	
	//float posX, posY, heading;
	Khepera::ReadPos((float*)&xx,(float*) &yy, (float*)&yyaw);
	
	
  d->pos.px = xx;
  d->pos.py = yy;
  d->pos.pa = yyaw;
  
  #else

  // calculate position data
  int pos_left, pos_right;
  Khepera::ReadPosCounter(&pos_left, &pos_right);
  
  //calculate variations of position of wheels
  int change_left = pos_left - last_lpos;
  int change_right = pos_right - last_rpos;
  
  //correction of encoders overflow
  if(change_left>60000)
  	change_left = (pos_left-65535)-last_lpos;
 	else if(change_left<-60000)
 		change_left = (pos_left)+(65535-last_lpos);
 	
 	if(change_right>60000)
  	change_right = (pos_right-65535)-last_rpos;
 	else if(change_right<-60000)
 		change_right = (pos_right)+(65535-last_rpos);
 	
 	//update last positions
  last_lpos = pos_left;
  last_rpos = pos_right;

  double transchange = ((double)change_left*geometry->odometry.k1 + (double)change_right*geometry->odometry.k2) * geometry->encoder_res / 2;
  double rotchange = ((double)change_left*geometry->odometry.k1 - (double)change_right*geometry->odometry.k2) * geometry->encoder_res / (geometry->odometry.wheelDist*geometry->odometry.k3);
  
  double dx,dy,Theta;

  Theta = rotchange;
  dx = transchange*cos(-yaw-Theta/2);
  dy = transchange*sin(-yaw-Theta/2);
  x += dx;
  y += dy;
  yaw += Theta;

	while (yaw > 2*M_PI) yaw -= 2*M_PI;
  while (yaw < 0) yaw += 2*M_PI;

  d->pos.px = x;
  d->pos.py = y;
  d->pos.pa = yaw;

  // correct angle to be in the [-pi, pi] radian range  
  if(d->pos.pa > M_PI)
  	d->pos.pa -=2*M_PI;

  d->pos.pa *= -1;
  
  #endif
  
  d->vel.px = 0;//trans_vel;
  d->vel.py = 0;//trans_vel;
  d->vel.pa = 0;//DTOR(rot_vel_deg);

}

/* this will set the odometry to a given position
 * ****NOTE: assumes that the arguments are in network byte order!*****
 *
 * returns: 
 */
int
Khepera::ResetOdometry()
{
  printf("Reset Odometry\n");
  
  #if CALCULATE_ODOMETRY == 0
  
  float Values[3];
  Values[0] = 0;
  Values[1] = 0;
  Values[2] = 0;
  //in this case, be use the command SET_POSITION
  if (Serial->KheperaCommandF('G',3,Values,0,NULL) < 0)
    return -1;
  
  #else
  int Values[2];
  Values[0] = 0;
  Values[1] = 0;
  
  //be use the command SET_ENCODERS
  if (Serial->KheperaCommand('O',2,Values,0,NULL) < 0)
    return -1;
    
  last_lpos = 0;
  last_rpos = 0;

  x=y=yaw=0;
  
  #endif
  
  player_position2d_data_t data;
  memset(&data,0,sizeof(player_position2d_data_t));
  Publish(position_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, &data, sizeof(data),NULL);

  return 0;
}

int
Khepera::ReadAllIR(player_ir_data_t* d)
{
  int * Values;

  Values = new int [geometry->ir.poses_count];
  int valAllIR;


  //nuestro robot devuelve solamente en un byte los datos, por eso no se utiliza el numero de ir para la cantidad de datos
  //a recibir, sino solamente 1 byte.
  
  if(Serial->KheperaCommand('N',0,NULL,1,&valAllIR) < 0)
  {
    //printf("fallo\n");
    return -1;	
  }

  		
  for (unsigned int i=0; i< geometry->ir.poses_count; ++i)
  {
    if(valAllIR & (1<<i))
    		Values[i] = 1;
    else
    		Values[i] = 0;
    
    //falta verificar si estos son los valores que van aqui
    d->voltages[i] = static_cast<short> (Values[i]);
  }
  delete [] Values;
  return 0;
}

/* this will set the desired speed for the given motor mn
 *
 * returns:
 */
int
Khepera::SetSpeed(int speed1, int speed2)
{

	int Values[2];
	Values[0] = speed1;
	Values[1] = speed2;

	return Serial->KheperaCommand('D',2,Values,0,NULL);

}

int 
Khepera::PlayJingle(int jingleIndex)
{
	return Serial->KheperaCommand('R',1,&jingleIndex,0,NULL);
}
/* reads the current speed of motor mn
 *
 * returns: the speed of mn
 */
int
Khepera::ReadSpeed(int * left,int * right)
{
	int Values[2];
	if (Serial->KheperaCommand('E',0,NULL,2,Values) < 0)
		return -1;
	*left = Values[0];
	*right = Values[1];
	return 0;
}



/* this will read the current value of the position counter
 * for motor mn
 *
 * returns: the current position for mn
 */
 
int
Khepera::ReadPos(float * pos1, float * pos2, float * pos3)
{
	float Values[3];
	if (Serial->KheperaCommandF('H',0,NULL,3,Values) < 0)
		return -1;
	
	*pos1 = Values[0];
	*pos2 = Values[1];
	*pos3 = Values[2];
	
	return 0;
}


int
Khepera::ReadPosCounter(int * pos1, int * pos2)
{
	int Values[2];
	if (Serial->KheperaCommand('S',0,NULL,2,Values) < 0)
		return -1;
	
	*pos1 = Values[0];
	*pos2 = Values[1];
	return 0;
}


int
Khepera::SetPos(float x, float y, float heading)
{
	float Values[3];
	Values[0] = x;
	Values[1] = y;
	Values[2] = heading;
	
	if (Serial->KheperaCommandF('G',3,Values,0,NULL) < 0)
		return -1;
	
	return 0;
	
}

int
Khepera::SetPosCounter(int posLeft, int posRight)
{
	int Values[2];
	Values[0] = posLeft;
	Values[1] = posRight;
	
	if (Serial->KheperaCommand('O',2,Values,0,NULL) < 0)
		return -1;
	
	return 0;
	
}


